#!/usr/bin/env python

import sys, rospy, cv2, time
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError


class RGBD_Image:
	def __init__(self):
		# 初始化类并从摄像头获取信息
		#rospy.init_node('photo', anonymous=False)
		image_topic = '/camera/rgb/image_raw'
		depth_topic = '/camera/depth/image_raw'
		image_sub = rospy.Subscriber(image_topic, Image, self.image_call_back)
		depth_sub = rospy.Subscriber(depth_topic, Image, self.depth_call_back)
		print('RGBD_Image Init.')
		rospy.sleep(3)


	def image_call_back(self, data):
		# 定义rgb图像回调函数
		try:
			bridge = CvBridge()
			self.rgb_image = bridge.imgmsg_to_cv2(data, 'bgr8')
			# Convert images to displayable data types.
		except:
			pass

	
	def depth_call_back(self, data):
		# 定义深度图像回调函数
		try:
			bridge = CvBridge()
			self.depth_image = bridge.imgmsg_to_cv2(data, '16UC1')
		except:
			pass


	def get_image(self):
		# 获取rgb图像
		return self.rgb_image


	def get_depth_image(self):
		# 获取深度图像
		return self.depth_image


if __name__ == '__main__':
	image = RGBD_Image()
	rospy.init_node('Image', anonymous=False)
	while True:
		src = image.get_depth_image()
		cv2.imshow('src', src)
		if 27 == cv2.waitKey(1):
			break

